/*
 * Scheduler.c
 *
 *  Created on: 2023年6月26日
 *      Author: Administrator
 */

#include "Scheduler.h"
#include "type.h"
#include <REGF51RC.H>
#include <intrins.h>
#include "oled.h"
#include "isr.h"
#include "pwm.h"
#include "encode.h"
#include "mpu6050.h"
#include "comFunction.h"
#include "motor.h"
#include "config.h"
#include <string.h>

// 4个任务占用44Byte
// 定时器计数占用4Byte
// 总共占用48Byte

// -10.7327  -12.1049   -2.5157
// -3.9482   -5.3236   -1.7600
float K[3] = {40.7327, -12.1049, 2.5157};

//////////////////////////////////////////////////////////////////////
//用户程序调度器
//////////////////////////////////////////////////////////////////////
static uint32_t SysRunTimeMs = 0;
static void Loop_1000Hz(void) //1ms执行一次
{
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
}

static void Loop_100Hz(void) //10ms执行一次
{
//////////////////////////////////////////////////////////////////////
    static int16_t x = 0, y = 0, z = 0;
    static int32_t X = 0, Z = 0, Y = 0, S1 = 0, S2 = 0;
    static volatile int32_t theta = 0, dotTheta = 0;
    static int32_t OUT;
    static int16_t count = 0;
    static float dotAngle = 0, angle = 0;
    static int8_t dir = 5;
    P0 ^= 0x80;
#if USE_MPU
    MPU_Get_Accelerometer(&x, &y, &z);
    X = (int32_t)x;
    Z = (int32_t)z;
    theta = (int32_t)(func_Qrsqrt(X * X + Z * Z) * X * 500) + (X * 500 / Z);
    MPU_Get_Gyroscope(&x, &y, &z);
    Y = (int32_t)y;
    dotTheta = Y;
#endif
#if USE_ENCODE
    S1 = encodeReadErase(0);
    S2 = encodeReadErase(1);
#endif
    count += dir;
    if (count > 950 || count < -950)
    {
        dir *= -1;
    }
    OUT = (int32_t)(K[0] * (S1 + S2) * 0.793) + (int32_t)(K[1] * theta + K[2] * dotTheta)/ 12;
#if USE_OLED
    // OLED_ShowInt(0, 2, S1, 6);
    // OLED_ShowInt(0, 3, S2, 6);
    OLED_ShowInt(40, 1, S1, 6);
    OLED_ShowInt(40, 2, S2, 6);
    OLED_ShowInt(40, 3, z, 6);
    OLED_ShowInt(40, 4, dotTheta, 6);
    OLED_ShowInt(40, 5, theta, 6);
    OLED_ShowInt(40, 6, OUT, 5);
#endif
#if USE_MOTOR
    motorSet(0, func_limit(func_outside_ab(OUT, -140, 70), 1000));
    motorSet(1, func_limit(func_outside_ab(OUT, -140, 70), 1000));
#endif
//////////////////////////////////////////////////////////////////////
}

static void Loop_10Hz(void) //100ms执行一次
{
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
}

static void Loop_1Hz(void) //1s执行一次
{
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
}

//////////////////////////////////////////////////////////////////////
//调度器时间函数
//////////////////////////////////////////////////////////////////////
void SysTick_Handler(void)
{
    SysRunTimeMs++;
}
uint32_t GetSysRunTimeMs(void)
{
    return SysRunTimeMs;
}

//////////////////////////////////////////////////////////////////////
//调度器初始化
//////////////////////////////////////////////////////////////////////
//系统任务配置，创建不同执行频率的“线程”
static sched_task_t sched_tasks[] =
{
		{Loop_1000Hz, 1000, 0, 0},
		{Loop_100Hz, 100, 0, 0},
		{Loop_10Hz, 10, 0, 0},
		{Loop_1Hz, 1, 0, 0},
};
//根据数组长度，判断线程数量
#define TASK_NUM (sizeof(sched_tasks) / sizeof(sched_task_t))

void Scheduler_Setup(void)
{
    uint8_t index = 0;
    //初始化任务表
    for (index = 0; index < TASK_NUM; index++)
    {
        //计算每个任务的延时周期数
        sched_tasks[index].interval_ticks = TICK_PER_SECOND / sched_tasks[index].rate_hz;
        //最短周期为1，也就是1ms
        if (sched_tasks[index].interval_ticks < 1)
        {
            sched_tasks[index].interval_ticks = 1;
        }
    }
}
//这个函数放到main函数的while(1)中，不停判断是否有线程应该执行
void Scheduler_Run(void)
{
    uint8_t index = 0;
    //循环判断所有线程，是否应该执行

    for (index = 0; index < TASK_NUM; index++)
    {
        //获取系统当前时间，单位MS
        uint32_t tnow = GetSysRunTimeMs();
        //进行判断，如果当前时间减去上一次执行的时间，大于等于该线程的执行周期，则执行线程
        if (tnow - sched_tasks[index].last_run >= sched_tasks[index].interval_ticks)
        {
            //更新线程的执行时间，用于下一次判断
            sched_tasks[index].last_run = tnow;
            //执行线程函数，使用的是函数指针
            sched_tasks[index].task_func();
        }
    }
}

void Scheduler_Init(void)
{
    Scheduler_Setup(); // 调度器初始化
    Timer2_Init();     // 定时器2初始化
}
